/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "RangingFileReader.hpp"
#include "owndebug.h"


int main(void){
  CRangingFileReader reader;
  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(1);

  reader.ReadFile("J2B2.MaCI_Ranging.Laser_07_02_2011_16:41:32.txt");


  std::string macigroup;
  std::string instance;
  if(reader.GetServiceInformation(macigroup,instance)){
    dPrint(1,"Got service information group '%s', instancename '%s'",
           macigroup.c_str(),
           instance.c_str());
  }else{
    dPrint(1,"Failed to get service information");
    exit(1);
  }
  MaCI::Ranging::TDeviceInformationLimits deviceLimits;
  if(reader.GetDeviceLimits(deviceLimits)){
    dPrint(1,"Got device limits min: %f, max: %f",deviceLimits.min_distance,deviceLimits.max_distance);

  }else{
    dPrint(1,"Failed to get device limits");
    exit(1);
  }
  MaCI::Ranging::TDeviceInformationPosition devicePosition;
  if(reader.GetDevicePosition(devicePosition)){
    dPrint(1,"Got device position (%f ; %f ; %f) heading (%f)",
           devicePosition.x,
           devicePosition.y,
           devicePosition.z,
           devicePosition.heading);
  }else{
    dPrint(1,"Failed to get device limits");
    exit(1);
  }
     
  std::list<gim::time> times;
  if(reader.GetTimes(times)){
    dPrint(1,"Got times... number of times %d",times.size());
  }else{
    dPrint(1,"Failed to get times ");
    exit(1);
  }
  
  gim::time begin,end;
  if(reader.GetTimeLimits(begin,end)){
    dPrint(1,"Got time limits.. begin %u %u, end %u %u",
           begin.seconds,
           begin.useconds,
           end.seconds,
           end.useconds);
           
  }else{
    dPrint(1,"Failed to get time limits");
    exit(1);
  }
  TRangingDataMap dataMap;
  if(reader.GetAllData(dataMap)){
    dPrint(1,"Got all data. Number of measurements %d",dataMap.size());
    
    
  }else{
    dPrint(1,"Failed to get all data");
    return false;
  }
  
  MaCI::Ranging::CRangingData data;
  if(reader.GetDistanceArray(data,end)){
    dPrint(1,"Got the last measurement");
    data.Print(1);

  }else{
    dPrint(1,"Failed to get the first measurement");
    return false;
  }
  
  
}
